#include "CameraCollegeZeroCopy.h"
#include <cstring>
#include <iostream>
#include <sys/time.h>

CameraCollegeZeroCopy::CameraCollegeZeroCopy(GX_DEV_HANDLE* _ptrDevice,ThreadSafeQueue<FrameData>* _ptrQueue,unsigned _bufferSize)
{
    std::ios::sync_with_stdio(false);
    ptrDevice=_ptrDevice;
    ptrQueue=_ptrQueue;
    pFrameBuffer=new PGX_FRAME_BUFFER[_bufferSize];
    pFrameBufferHandle=new PGX_FRAME_BUFFER[_bufferSize];
    FrameBufferSize=_bufferSize;

    HandleThread=std::thread(&CameraCollegeZeroCopy::HandleFunc,this);
}

CameraCollegeZeroCopy::~CameraCollegeZeroCopy()
{
    ThreadExitFlag.store(true);
    if(HandleThread.joinable())
        HandleThread.join();
    delete[] pFrameBuffer;
}

//static struct timeval tv;
//static struct timezone tz;

void CameraCollegeZeroCopy::HandleFunc()
{
    uint32_t Icount;
    while(!ThreadExitFlag.load())
    {
        //std::cout<<"LOGD NewIn Loop: \n";
        if(GXDQAllBufs(*ptrDevice,pFrameBuffer,FrameBufferSize,&Icount,500)!=0)
        {
            //std::cout<<"  Fail Get Image!!!\n";
            continue;
        }
        std::memcpy(pFrameBufferHandle,pFrameBuffer,FrameBufferSize*sizeof(PGX_FRAME_BUFFER));
        GXQAllBufs(*ptrDevice);
        //std::cout<<"  Get New Image, "<<Icount<<'-'<<FrameBufferSize<<'\n';
        //gettimeofday(&tv,&tz);
        //std::cout<<"  Entry time: "<<tv.tv_usec<<'\n';
        CollegeImageSize.store(Icount);
        if(firstImage && pFrameBufferHandle[0]!=nullptr)
        {
            StartTime=pFrameBufferHandle[0]->nTimestamp/125000;
            firstImage=false;
        }
        for(int i=0;i<Icount;i++)
        {
            if(pFrameBufferHandle[i]!=nullptr && pFrameBufferHandle[i]->nStatus==GX_FRAME_STATUS_SUCCESS)
            {
                unsigned char* ImgDataPtr=new unsigned char[pFrameBufferHandle[i]->nImgSize];
                std::memcpy(ImgDataPtr,pFrameBufferHandle[i]->pImgBuf,pFrameBufferHandle[i]->nImgSize);
                ptrQueue->push({
                    ImgDataPtr,
                    pFrameBufferHandle[i]->nWidth,
                    pFrameBufferHandle[i]->nHeight,
                    pFrameBufferHandle[i]->nPixelFormat,
                    pFrameBufferHandle[i]->nFrameID,
                    pFrameBufferHandle[i]->nTimestamp/125000-StartTime
                });
            }
        }
        //gettimeofday(&tv,&tz);
        //std::cout<<"  Finish Time: "<<tv.tv_usec<<'\n';
    }
}
